/*
 * CruiseLocationTool.cpp
 *
 *  Created on: 2017年5月23日
 *      Author: roswiki
 */

#include <tf/transform_listener.h>
#include <geometry_msgs/PoseStamped.h>
#include "rviz/display_context.h"

#include <katrina_gui/CruiseLocationTool.h>

namespace Katrina
{

    CruiseLocationTool::CruiseLocationTool()
    {
//  shortcut_key_ = 'k';
    }

    void CruiseLocationTool::onInitialize()
    {
        PoseTool::onInitialize();
        setName("Katrina Cruise Location");
        updateTopic();
    }

    void CruiseLocationTool::updateTopic()
    {
        pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/Katrina/MouseCruiseLocation", 1);
    }

    void CruiseLocationTool::onPoseSet(double x, double y, double theta)
    {
        bool ok;
        std::string name = QInputDialog::getText(0, QString::fromUtf8("添加新的巡航位置"), QString::fromUtf8("位置名称:"),
                                                 QLineEdit::Normal, "", &ok).toStdString();
        if (!ok)
            return;
        std::string fixed_frame = context_->getFixedFrame().toStdString();
        tf::Quaternion quat;
        quat.setRPY(0.0, 0.0, theta);
        tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose(quat, tf::Point(x, y, 0.0)), ros::Time::now(), name);
        geometry_msgs::PoseStamped goal;
        tf::poseStampedTFToMsg(p, goal);
        ROS_INFO(
                "Setting cruise location: Frame:%s, Position(%.3f, %.3f, %.3f), Orientation(%.3f, %.3f, %.3f, %.3f) = Angle: %.3f\n",
                fixed_frame.c_str(), goal.pose.position.x, goal.pose.position.y, goal.pose.position.z,
                goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w,
                theta);
        pub_.publish(goal);
    }

}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(Katrina::CruiseLocationTool, rviz::Tool)
